#ifndef __CTRL_H__
#define __CTRL_H__

#include <iostream>
#include <cstdint>
#include <thread>
#include <mutex>
#include <chrono>
#include "commManager.h"

/*数据包相关命令*/
#define CMD_TEST            0x01        // 用于测试连通性
#define CMD_MOTOR_POSE      0x02        // 控制电机位置data长度固定为5 data[0]-电机位号:1-6 data[1]-data[4]:需要转动到的绝对步进角(uint32_t大端)
#define CMD_MOTOR_RESET     0x03        // 复位所有电机
#define CMD_MOTOR_GETPOS    0x04        // 更新电机位置data长度固定为6 第一位是电机编号 第二位是符号位0正1负, 最后四位表示一个u32 (大端)
#define CMD_MOTOR_SERVO     0x05        // 控制舵机 data长度固定为2 data[0]-舵机位号:7/8 ,data[1]:舵机转动的角度
#define CMD_MOTOR_ERR       0xff        // 电机发生错误

#if 0
#define DBG(fmt, ...) do {\
    auto now = std::chrono::system_clock::now(); \
    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count(); \
    auto seconds = ms / 1000; \
    auto milliseconds = ms % 1000; \
    std::time_t now_c = std::chrono::system_clock::to_time_t(now); \
    char time_str[26]; \
    std::strftime(time_str, sizeof(time_str), "%Y-%m-%d %H:%M:%S", std::localtime(&now_c));\
    std::cout << "[" << time_str << " " << seconds << " seconds " << milliseconds << " milliseconds]: "; \
    std::printf(fmt, ##__VA_ARGS__); \
    } while (0)


#define WAIT_INPUT_YES() \
do { \
    std::string userInput; \
    while (true) { \
        std::cout << "请输入 'y' 或 'Y' 以退出循环: "; \
        std::cin >> userInput; \
        std::transform(userInput.begin(), userInput.end(), userInput.begin(), ::tolower); \
        if (userInput.find('y') != std::string::npos) { \
            break; \
        }\
    } \
} while (false)

#else
#define DBG(fmt, ...)
#define WAIT_INPUT_YES() 
#endif

/*该枚举类型用于PCBmachine成员函数访问Flage[]*/
typedef enum {
    IMG_SIG = 0,                /*true表示:可以开始检测pcb*/
    M1_SIG = 1,                 /*1号电机是否复位*/
    M2_SIG = 2,                 /*2号电机否复位复位*/
    STEP4_SIG = 3,              /*true表示:升降台复位, 3456号电机和两个舵机均复位*/
    PCB_ON_AssemblyLine = 4,    /*有无PCB在2号电机处*/
    PCB_ON_Platform = 5,        /*有无PCB在升降平台处 */
}PCBFlage;

/*该结构体用于存储电机角度*/
typedef struct {
    double M1;
    double M2;
    double M3;
    double M4;
    double M5;
    double M6;
    double S1;
    double S2;
}MotorPose_t;

class PCBmachine: public commManager{
public:
    PCBmachine();
    ~PCBmachine();
    void place(int x, int y);   // 设置本pcb要放置的坐标
    bool GetImgSignal();        // 是否可以开始本张PCB的检测
private:
    bool PCBmachine_ok;
    MotorPose_t MotorPose;
    bool Flage[6];
    void Step1();
    void Step2();
    void Step3(int x, int y);
    std::mutex mutex_Flage;
    std::mutex mutex_Step[3];
    bool SetFlage(PCBFlage id, bool state); // 设置Flage[id]状态 返回state
    bool GetFlage(PCBFlage id, bool& state); // 获取Flage[id]状态 
    bool GetFlage(PCBFlage id); // 获取Flage[id]状态 
    void U32_TO_U8ARR(uint8_t arr[], uint32_t count); // 将32位无符号整数 存在长度为4的u8数组中(大端), 要求arr的长度>=4
    uint32_t angle_to_step(double angle); // 角度换算成步数
    double step_to_angle(uint32_t step); // 步数换算成脚步
    void delay_ms(int ms);

public: // (测试用, 后面删掉)

    /*一些功能函数*/
    void reset_platform(bool wait = false); // 控制升降台复位至水平
    void wait_for_motor_movement(int id, uint32_t target_step, bool resend_data = true , uint32_t send_interval = 100, uint32_t err = 50);// 等待电机运动到指定步进


    /*使用绝对步进控制对应电机 , 1细分下一圈为200个步进 16细分下一圈为3200个步进*/
    void CtrlM1(uint32_t step);
    void CtrlM2(uint32_t step);
    void CtrlM3_4(uint32_t step);
    void CtrlM5(uint32_t step);
    void CtrlM6(uint32_t step);
    void CtrlM7(uint8_t angle);
    void CtrlM8(uint8_t angle);

    /*控制指令回调函数*/
    int CmdTest(std::vector<uint8_t>& data);
    int CmdPose(std::vector<uint8_t>& data);
    int CmdErr(std::vector<uint8_t>& data);
};


#endif